/*
 * SailbotMK1 main.cpp
 *
 *  Created on: 2010-06-09
 *      Author: brianclaus
 *      TODO create motor and encoder as objects
 */



#include "common.h"



//serial responder states
enum { main_sail_speed='a',
	jib_sail_speed,
	rudder_speed,
	main_sail_state,
	jib_sail_state,
	rudder_state,
	main_sail_position,			//g
	jib_sail_position,
	rudder_position,
	main_sail_setpoint,      		//j
	jib_sail_setpoint,				//k
	rudder_setpoint,				//l
	main_sail_pid_params,			//m
	jib_sail_pid_params,			//n
	rudder_pid_params,				//o
	read_measured_positions};		//p

static int mainPIDoutput;
static int jibPIDoutput;
static int rudderPIDoutput;
static unsigned long timer;

// This code is a hack
// It is required to compile print functions in eclipse
extern "C" void __cxa_pure_virtual(void);
void __cxa_pure_virtual(void) {}

Motor main_motor(main_IN1,main_IN2,main_PWM,motor_stop);
Motor jib_motor(jib_IN1,jib_IN2,jib_PWM,motor_stop);
Motor rudder_motor(rudder_IN1,rudder_IN2,rudder_PWM,motor_stop);
Encoder main_encoder(main_CHA,main_CHB,main_start<<main_scale_factor);
Encoder jib_encoder(jib_CHA,jib_CHB,jib_start<<jib_scale_factor);
Encoder rudder_encoder(rudder_CHA,rudder_CHB,rudder_start<<rudder_scale_factor);
pid  main_pid(main_pid_p_gain,main_pid_i_gain,main_pid_d_gain, main_pid_i_error_limit,main_pid_max_commanded_value,main_pid_min_commanded_value,main_pid_output_limit,main_start);
pid  jib_pid(jib_pid_p_gain,jib_pid_i_gain,jib_pid_d_gain,jib_pid_i_error_limit,jib_pid_max_commanded_value,jib_pid_min_commanded_value,jib_pid_output_limit, jib_start);
pid  rudder_pid(rudder_pid_p_gain,rudder_pid_i_gain,rudder_pid_d_gain,rudder_pid_i_error_limit,rudder_pid_max_commanded_value,rudder_pid_min_commanded_value,rudder_pid_output_limit,rudder_start);

void receiveEvent(int howMany);

void main_init(void){
	// The init() function is defined in the Arduino library.
	// It initializes the timers and the ADC on the ATmega328p.
	init();
	Wire.begin(MOTORBOARDADDRESS);
	Wire.onReceive(receiveEvent); // register event
	Serial.begin(57600);  //for some reason this is actually 115200
	TCCR1B = TCCR1B & 0b11111010; //set pin 9 and 10 pwm to 3.94kHz
	TCCR2B &= ~_BV(CS22); //disable cs22 and enable cs21 make clock scaler 2 (divisor 8)
	TCCR2B |= _BV(CS21); //set pin 3 and 11 pwm to 3.94kHz


	debug("main init");
}

void motor_init(void){

	main_motor.init();
	jib_motor.init();
	rudder_motor.init();

}

void encoder_init(void){
	cli();
	main_encoder.init();
	jib_encoder.init();
	rudder_encoder.init();
	sei();
}

void pid_init(void){
	main_pid.init();
}



int main(void){

    pinMode(8,OUTPUT);
	main_init();
	motor_init();
	encoder_init();
	//should read commands from main controller
	//and update the motor states/speeds accordingly



	while(1){ //main loop at 80 Hz
		if((millis()-timer)>=25){



			mainPIDoutput = main_pid.computePID(main_encoder.getPosition()>>main_scale_factor);
			if( mainPIDoutput > main_pwm_duty_deadband){
				main_motor.backward();
				main_motor.setSpeed(mainPIDoutput);
				debug("main position %ld, speed %d", main_encoder.getPosition()>>main_scale_factor,mainPIDoutput);
			}
			else if(mainPIDoutput < -main_pwm_duty_deadband) {
				main_motor.forward();
				main_motor.setSpeed(-mainPIDoutput);
				debug("main position %ld speed %d", main_encoder.getPosition()>>main_scale_factor,mainPIDoutput);
			}
			else{
				//debug("main position %ld, speed %d", main_encoder.getPosition()>>main_scale_factor,mainPIDoutput);
				main_motor.brake();
			}

			jibPIDoutput = jib_pid.computePID(jib_encoder.getPosition()>>jib_scale_factor);
			if( jibPIDoutput > jib_pwm_duty_deadband){
				jib_motor.backward();
				jib_motor.setSpeed(jibPIDoutput);
				debug("jib position %ld, speed %d", jib_encoder.getPosition()>>jib_scale_factor,jibPIDoutput);
			}
			else if(jibPIDoutput < -jib_pwm_duty_deadband) {
				jib_motor.forward();
				jib_motor.setSpeed(-jibPIDoutput);
				debug("jib position %ld speed %d", jib_encoder.getPosition()>>jib_scale_factor,jibPIDoutput);
			}
			else{
				//debug("jib position %ld, speed %d", jib_encoder.getPosition()>>jib_scale_factor,jibPIDoutput);
				jib_motor.brake();
			}

			rudderPIDoutput = rudder_pid.computePID(rudder_encoder.getPosition()>>rudder_scale_factor);
			if( rudderPIDoutput > rudder_pwm_duty_deadband){
				rudder_motor.backward();
				rudder_motor.setSpeed(rudderPIDoutput);
				debug("rudder position %ld, speed %d", rudder_encoder.getPosition()>>rudder_scale_factor,rudderPIDoutput);
			}
			else if(rudderPIDoutput < -rudder_pwm_duty_deadband) {
				rudder_motor.forward();
				rudder_motor.setSpeed(-rudderPIDoutput);
				debug("rudder position %ld speed %d", rudder_encoder.getPosition()>>rudder_scale_factor,rudderPIDoutput);
			}
			else{
				//debug("rudder position %ld, speed %d", rudder_encoder.getPosition()>>rudder_scale_factor,rudderPIDoutput);
				rudder_motor.brake();
			}
		}

		digitalWrite(8,LOW);
	}//while


}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany)
{
	int firstByte, secondByte,thirdByte;
	int checksumCalc = 0, checksumRead = 0;
	uint8_t bytetoRead[6];
    uint8_t bytetoSend[4];
    unsigned int checksumSend=0;
    int i;
	digitalWrite(8,HIGH);
	checksumCalc = 0;

	if (Wire.available() > 0) {
		//Checksum calc implementation requires all motor commands to be three bytes

		digitalWrite(8,LOW);
		for(i=0;i<3;i++){				//read bytes less checksum
			bytetoRead[i]=(uint8_t)Wire.receive();
			checksumCalc += (uint8_t)bytetoRead[i];
		}
		bytetoRead[3]=Wire.receive();
		bytetoRead[4]=Wire.receive();
		checksumRead = (unsigned int)bytetoRead[3] + (((unsigned int)bytetoRead[4])<<8);

		firstByte = bytetoRead[0]; 	//first byte specifies which motor/action
		secondByte = bytetoRead[1]; 	//Second value of action if any (PWM)
		thirdByte = bytetoRead[2]; 	//third value of action if any (PWM)
		if(checksumRead == checksumCalc){//if checksums do not agree throw away data
			digitalWrite(8,HIGH);
			switch (firstByte) {
			case main_sail_speed:   //set main_sail pwm
				debug("main sail speed %d",secondByte);
				main_motor.setSpeed(secondByte);
				break;
			case jib_sail_speed:   //set jib_sail pwm
				debug("jib sail speed %d",secondByte);
				jib_motor.setSpeed(secondByte);
				break;
			case rudder_speed:   //set rudder pwm
				debug("rudder speed %d",secondByte);
				rudder_motor.setSpeed(secondByte);
				break;
			case main_sail_state:   //set main_sail state

				if(secondByte!=main_motor.getMotorState()){ //if different from current change
					switch(secondByte){
					case motor_forward:
						main_motor.forward();
						break;
					case motor_backward:
						main_motor.backward();
						break;
					case motor_brake:
						main_motor.brake();
						break;
					case motor_stop:
						main_motor.stop();
						break;
					default:
						break;//do nothing should not be here
					}
				}
				debug("main sail state %d",main_motor.getMotorState());
				break;
			case jib_sail_state:   //set jib_sail state
				debug("jib sail state %d",secondByte);
				if(secondByte!=jib_motor.getMotorState()){ //if different from current change
					switch(secondByte){
					case motor_forward:
						jib_motor.forward();
						break;
					case motor_backward:
						jib_motor.backward();
						break;
					case motor_brake:
						jib_motor.brake();
						break;
					case motor_stop:
						jib_motor.stop();
						break;
					default:
						break;//do nothing should not be here
					}
				}
				break;
			case rudder_state:   //set rudder state
				debug("rudder state %d",secondByte);
				if(secondByte!=rudder_motor.getMotorState()){ //if different from current change
					switch(secondByte){
					case motor_forward:
						rudder_motor.forward();
						break;
					case motor_backward:
						rudder_motor.backward();
						break;
					case motor_brake:
						rudder_motor.brake();
						break;
					case motor_stop:
						rudder_motor.stop();
						break;
					default:
						break;//do nothing should not be here
					}
				}
				break;
			case main_sail_position:
				debug("main motor position %ld error %d direction %d",main_encoder.getPosition(),main_encoder.getErrors(),main_encoder.getDirection());
				main_encoder.setPosition((thirdByte<<8)+secondByte);
				break;
			case jib_sail_position:
				debug("jib motor position %ld error %d direction %d",jib_encoder.getPosition(),jib_encoder.getErrors(),jib_encoder.getDirection());
				jib_encoder.setPosition((thirdByte<<8)+secondByte);
				break;
			case rudder_position:
				debug("rudder motor position %ld error %d direction %d",rudder_encoder.getPosition(),rudder_encoder.getErrors(),rudder_encoder.getDirection());
				//rudder_encoder.setPosition((thirdByte<<8)+secondByte);
				break;
			case main_sail_setpoint:
				debug("main sail setpoint %d",(thirdByte<<8)+secondByte);
				main_pid.setPIDSetpoint((thirdByte<<8)+secondByte); //each setpoint increment is one revolution of encoder wheel
				break;
			case jib_sail_setpoint:
				debug("jib sail setpoint %d",(thirdByte<<8)+secondByte);

				jib_pid.setPIDSetpoint((thirdByte<<8)+secondByte); //each setpoint increment is one revolution of encoder wheel
				break;
			case rudder_setpoint:
				debug("rudder setpoint %d",(thirdByte<<8)+secondByte);

				rudder_pid.setPIDSetpoint((thirdByte<<8)+secondByte); //each setpoint increment is one revolution of encoder wheel
				break;
			case main_sail_pid_params:
				debug("set main sail param %d with %d",secondByte, thirdByte);
				switch(secondByte){
					case tune_pid_p_gain:
						main_pid.setPIDPgain(thirdByte);
						break;
					case tune_pid_i_gain:
						main_pid.setPIDIgain(thirdByte);
						break;
					case tune_pid_d_gain:
						main_pid.setPIDDgain(thirdByte);
						break;
					case tune_pid_ierror_limit:
						main_pid.setPIDIerrorlimit(thirdByte);
						break;
					case tune_pid_output_limit:
						main_pid.setPIDOutputlimit(thirdByte);
						break;
					case tune_pid_output_min:
						main_pid.setPIDOutputmin(thirdByte);
						break;
					default:
						break;//do nothing should not be here
					}
				break;
			case jib_sail_pid_params:
				debug("set jib sail param %d with %d",secondByte, thirdByte);
				switch(secondByte){
					case tune_pid_p_gain:
						jib_pid.setPIDPgain(thirdByte);
						break;
					case tune_pid_i_gain:
						jib_pid.setPIDIgain(thirdByte);
						break;
					case tune_pid_d_gain:
						jib_pid.setPIDDgain(thirdByte);
						break;
					case tune_pid_ierror_limit:
						jib_pid.setPIDIerrorlimit(thirdByte);
						break;
					case tune_pid_output_limit:
						jib_pid.setPIDOutputlimit(thirdByte);
						break;
					case tune_pid_output_min:
						jib_pid.setPIDOutputmin(thirdByte);
						break;
					default:
						break;//do nothing should not be here
					}
				break;
			case rudder_pid_params:
				debug("set rudder param %d with %d",secondByte, thirdByte);
				switch(secondByte){
					case tune_pid_p_gain:
						rudder_pid.setPIDPgain(thirdByte);
						break;
					case tune_pid_i_gain:
						rudder_pid.setPIDIgain(thirdByte);
						break;
					case tune_pid_d_gain:
						rudder_pid.setPIDDgain(thirdByte);
						break;
					case tune_pid_ierror_limit:
						rudder_pid.setPIDIerrorlimit(thirdByte);
						break;
					case tune_pid_output_limit:
						rudder_pid.setPIDOutputlimit(thirdByte);
						break;
					case tune_pid_output_min:
						rudder_pid.setPIDOutputmin(thirdByte);
						break;
					default:
						break;//do nothing should not be here
					}
				break;
			case read_measured_positions:
				  checksumSend = 0;
				  for(i=0;i<4;i++){
					bytetoSend[i]=main_encoder.getPosition()>>(i*8);
					checksumSend += (unsigned int)bytetoSend[i];
				  }
				  Wire.send(bytetoSend,sizeof(bytetoSend));

				  for(i=0;i<4;i++){
					bytetoSend[i]=jib_encoder.getPosition()>>(i*8);
					checksumSend += (unsigned int)bytetoSend[i];
				  }
				  Wire.send(bytetoSend,sizeof(bytetoSend));

				  for(i=0;i<4;i++){
					bytetoSend[i]=(rudder_encoder.getPosition()>>rudder_scale_factor)>>(i*8);
					checksumSend += (unsigned int)bytetoSend[i];
				  }
				  Wire.send(bytetoSend,sizeof(bytetoSend));

				  for(i=0;i<2;i++){
					bytetoSend[i]=main_pid.getPIDOutputlimit();
					checksumSend += (unsigned int)bytetoSend[i];
				  }
				  Wire.send(bytetoSend,2);

				  for(i=0;i<2;i++){
					bytetoSend[i]=jib_pid.getPIDOutputlimit();
					checksumSend += (unsigned int)bytetoSend[i];
				  }
				  Wire.send(bytetoSend,2);

				  for(i=0;i<2;i++){
					bytetoSend[i]=rudder_pid.getPIDOutputlimit();
					checksumSend += (unsigned int)bytetoSend[i];
				  }
				  Wire.send(bytetoSend,2);

				  for(i=0;i<2;i++){
					bytetoSend[i]=checksumSend>>(i*8);
				  }
				  Wire.send(bytetoSend,2);

				break;
			default:
				break;
			}//switch
			digitalWrite(8,HIGH);
		}//if
	}//if

}


ISR(PCINT0_vect)
 {
	cli(); //disable interrupts
	rudder_encoder.encoderISR();
	sei(); //enable interrupts
}

ISR(PCINT1_vect){
	cli(); //disable interrupts
	jib_encoder.encoderISR();
	sei(); //enable interrupts
}
ISR(PCINT2_vect){
	cli(); //disable interrupts
	main_encoder.encoderISR();
	sei(); //enable interrupts
}

